/*
 * AStar.h
 *
 *  Created on: Mar 13, 2010
 *      Author: Vu Phuong Hoang
 */

#ifndef ASTARH
#define ASTARH

#include <queue>
#include "Allocation.h"
#include "PathFinder.h"

///////////////////////////////////////////////////
//A* State
///////////////////////////////////////////////////
enum AStarState {
	NEW, OPEN, CLOSE
};

///////////////////////////////////////////////////
//A* Tracer
///////////////////////////////////////////////////
class AStarTracer {
public:
	Coordinate kCurrent, kPrevious;
	float fCost, fHeuristic;
	AStarTracer(Coordinate current, Coordinate previous, float cost = 0, float heuristic = 0) :
		kCurrent(current), kPrevious(previous), fCost(cost), fHeuristic(heuristic) {
	}
};

bool operator<(AStarTracer &a, AStarTracer &b) {
	if (a.fCost > b.fCost) {
		return true;
	} else if (a.fCost == b.fCost) {
		if (a.fHeuristic > b.fHeuristic) {
			return true;
		}
	}
	return false;
}

///////////////////////////////////////////////////
//A*
///////////////////////////////////////////////////
template<class T>
class AStar: public PathFinder<T> {
protected:
	static const int around[8][2];
	bool bPathExist;
	AStarState** ppeStateMap;

	std::list<Coordinate>* tracePath(std::list<AStarTracer>);
	bool initStateMap();
	bool isNew(unsigned int x, unsigned int y);
	float getCost(Coordinate from, Coordinate to);
public:
	float (*pfDistanceEstimator)(Coordinate, Coordinate);
	float (*pfGetWeight)(T);
	AStar(TerrainMap<T> map, float(*heuristicFunction)(Coordinate, Coordinate), float(*weightExtractor)(T)) :
		PathFinder<T> (map), pfDistanceEstimator(heuristicFunction), pfGetWeight(weightExtractor) {
		initStateMap();
	}
	std::list<Coordinate>* findPath(Coordinate from, Coordinate to);
	bool isPathExist();
};

template<class T>
const int AStar<T>::around[8][2] = { { -1, -1 }, { -1, 1 }, { 1, -1 }, { 1, 1 }, { 1, 0 }, { 0, -1 }, { 0, 1 },
		{ 1, 0 } };

template<class T>
bool AStar<T>::initStateMap() {
	return newMatrix(ppeStateMap, kMap.getRows(), kMap.getColumns(), NEW);
}

template<class T>
bool AStar<T>::isNew(unsigned int x, unsigned int y) {
	if (isInMap(x, y)) {
		if (ppeStateMap[x][y] == NEW) {
			return true;
		}
	}
	return false;
}

template<class T>
float AStar<T>::getCost(Coordinate from, Coordinate to) {
	return pfGetWeight(kMap[to.x][to.y]);
}

template<class T>
std::list<Coordinate>* AStar<T>::findPath(Coordinate from, Coordinate to) {
	kStart = from;
	kFinish = to;
	bPathExist = false;
	std::priority_queue<AStarTracer> openList;
	std::list<AStarTracer> closeList;

	openList.push(AStarTracer(from, from));

	while (!openList.empty()) {
		AStarTracer current = openList.top();
		openList.pop();
		closeList.push_back(current);
		if (isFinish(current.kCurrent)) {
			bPathExist = true;
			break;
		}
		unsigned int x = current.kCurrent.x, y = current.kCurrent.y;
		unsigned int newX, newY;
		for (unsigned int i = 0; i < 8; ++i) {
			newX = x + around[i][0];
			newY = y + around[i][1];
			if (isNew(newX, newY)) {
				Coordinate nextStep(newX, newY);
				float cost = current.fCost + getCost(current.kCurrent, nextStep);
				float heuristic = pfDistanceEstimator(nextStep, to);
				closeList.push_back(AStarTracer(nextStep, current.kCurrent, cost, heuristic));
			}
		}
	}

	return tracePath();
}

template<class T>
bool AStar<T>::isPathExist() {
	return bPathExist;
}

template<class T>
std::list<Coordinate>* AStar<T>::tracePath(std::list<AStarTracer>) {
	if (bPathExist) {
		//In progress
		return NULL;
	} else {
		return NULL;
	}
}

#endif /* ASTARH */
